#include "usr_inc.h"

/* 定义底盘模式 */
// ChassisMode_t chassisMode = YAW_MODE;
ChassisMode_t chassisMode = JOY_MODE;

/* 定义定位仪实例 */
/* 定义走点pid实例 */
Pid_t pointPid;
PoseMsg_t startPoint = {0};
PoseMsg_t endPoint = {0};
float dTheta = 0.0;

/* 定义角度环pid实例 */
Pid_t yawVelPid;
Pid_t yawPosPid;

Pid_t xVelPid;
Pid_t yVelPid;
Pid_t VelPid;
/* 定义卡尔曼滤波实体 */
KFP Mapan_x_Kalman;
KFP Mapan_y_Kalman;
KFP Radar_x_Kalman;
KFP Radar_y_Kalman;

float Merge_Loc_X;
float Merge_Loc_Y;
/**
 * @brief 初始化所有底盘相关的实例
 */
void ALL_CHASSIS_Init(void)
{
    PidInit_t pidInit = {0};

    pidInit.kp = 3.9;
    pidInit.ki = 0.0;
    pidInit.kd = 180.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 3.0 / 3;
    pidInit.maxOutput = 3.0;
    pidInit.minOutput = -3.0;
    PID_Init(&pointPid, &pidInit);

    pidInit.kp = 1.75;
    pidInit.ki = 0.00025;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 3.0 / 3;
    pidInit.maxOutput = 3.0;
    pidInit.minOutput = -3.0;
    PID_Init(&yawVelPid, &pidInit);

    pidInit.kp = 0.1;
    pidInit.ki = 0.0;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 1.5 / 3;
    pidInit.maxOutput = 1.5;
    pidInit.minOutput = -1.5;
    PID_Init(&yawPosPid, &pidInit);

    pidInit.kp = 5.0;
    pidInit.ki = 0.09;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 4.5 / 3;
    pidInit.maxOutput = 4.5;
    pidInit.minOutput = -4.5;
    PID_Init(&xVelPid, &pidInit);

    pidInit.kp = 5.0;
    pidInit.ki = 0.09;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 4.5 / 3;
    pidInit.maxOutput = 4.5;
    pidInit.minOutput = -4.5;
    PID_Init(&yVelPid, &pidInit);

    pidInit.kp = 2.0;
    pidInit.ki = 0.00;
    pidInit.kd = 0.0;
    pidInit.form = PID_POSITIONAL_FORM;
    pidInit.integralLimit = 0.8 / 3;
    pidInit.maxOutput = 0.8;
    pidInit.minOutput = -0.8;
    PID_Init(&VelPid, &pidInit);
}

void StartJoyExe(void *argument)
{
    float lpfVelX = 0.0;
    float lpfVelY = 0.0;
    float lpfOmega = 0.0;

    for (;;)
    {
        VelMsg_t velMsg = {0};
        JoyStickMsg_t joyMsg = {0};
        if (chassisMode == JOY_MODE || chassisMode == YAW_MODE)
        {
            if (xQueueReceive(joyMsgQueueHandle, &joyMsg, 1000) == pdTRUE)
            {
                lpfVelX += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.005 * 25))) * (joyMsg.leftX * 0.0018 - lpfVelX);    // 采样周期5ms, 截止频率25Hz
                lpfVelY += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.005 * 25))) * (joyMsg.leftY * 0.0018 - lpfVelY);    // 采样周期5ms, 截止频率25Hz
                lpfOmega += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.005 * 100))) * (joyMsg.rightY * 0.003 - lpfOmega); // 采样周期5ms, 截止频率100Hz
                // lpfVelX += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.005 * 25))) * (joyMsg.leftX * 0.0030 - lpfVelX);    // 采样周期5ms, 截止频率25Hz
                // lpfVelY += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.005 * 25))) * (joyMsg.leftY * 0.0030 - lpfVelY);    // 采样周期5ms, 截止频率25Hz
                // lpfOmega += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.005 * 100))) * (joyMsg.rightY * 0.003 - lpfOmega); // 采样周期5ms, 截止频率100Hz

                sCurveVx.speedTar = joyMsg.leftX * 0.0015;
                sCurveVy.speedTar = joyMsg.leftY * 0.0015;

                SCurveProcess(&sCurveVx);
                SCurveProcess(&sCurveVy);

                velMsg.velX = sCurveVx.out;
                velMsg.velY = sCurveVy.out;

                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                velMsg.velX = lpfVelX * cosf(locMsg.posYaw / 180.0 * M_PI) + lpfVelY * sinf(locMsg.posYaw / 180.0 * M_PI);
                velMsg.velY = (-1) * lpfVelX * sinf(locMsg.posYaw / 180.0 * M_PI) + lpfVelY * cosf(locMsg.posYaw / 180.0 * M_PI);
                velMsg.omega = lpfOmega;
            }
            // printf("%d,%f,%f\r\n",chassisMode,velMsg.velX,velMsg.velY);

        }
        xQueueOverwrite(chassisJoyVelMsgBoxHandle, &velMsg);
    }
}

void StartChassisPointExe(void *argument)
{
    PID_SetTarget(&pointPid, 0);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        VelMsg_t velMsg = {0};

        // SCurveProcess(&sCurve0);
        // SCurveProcess(&sCurve1);
        SCurveProcess(&sCurveM);

        if (chassisMode == POINT_MODE)
        {
            LocaterMsg_t locMsg;
            xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

            // float errX = sCurve0.out - locMsg.posX;
            // float errY = sCurve1.out - locMsg.posY;
            float errX = startPoint.posX + sCurveM.out * cosf(dTheta) - locMsg.posX;
            float errY = startPoint.posY + sCurveM.out * sinf(dTheta) - locMsg.posY;
            float distance = -hypotf(errX, errY);
            PID_Calculate(&pointPid, distance);

            if (fabs(distance) <= 0.01 )
            {
                velMsg.velX = 0.0;
                velMsg.velY = 0.0;
            }
            else
            {
                float v = PID_GetOutput(&pointPid);
                float rawX = v * cosf(atan2f(errY, errX));
                float rawY = v * sinf(atan2f(errY, errX));
                velMsg.velX = rawX * cosf(locMsg.posYaw / 180.0 * M_PI) + rawY * sinf(locMsg.posYaw / 180.0 * M_PI);
                velMsg.velY = (-1) * rawX * sinf(locMsg.posYaw / 180.0 * M_PI) + rawY * cosf(locMsg.posYaw / 180.0 * M_PI);
            }
            // printf("%f,%f\r\n",startPoint.posX + sCurveM.out * cosf(dTheta), startPoint.posY + sCurveM.out * sinf(dTheta));
            // xQueueOverwrite(chassisPointVelMsgBoxHandle, &velMsg);
        }
        xQueueOverwrite(chassisPointVelMsgBoxHandle, &velMsg);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisYawExe(void *argument)
{
    const float posDeadzone = 0.8;

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        VelMsg_t velMsg = {0};
        if (chassisMode == YAW_MODE || chassisMode == POINT_MODE)
        {
            LocaterMsg_t locMsg;
            xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

            const float posErr = fabs(locMsg.posYaw - PID_GetTarget(&yawPosPid));

            PID_Calculate(&yawPosPid, locMsg.posYaw);
            if (posErr <= posDeadzone)
            {
                PID_SetTarget(&yawVelPid, 0.0);
            }
            else
            {
                PID_SetTarget(&yawVelPid, PID_GetOutput(&yawPosPid));
            }

            PID_Calculate(&yawVelPid, locMsg.velYaw);
            if (posErr <= posDeadzone)
            {
                velMsg.omega = 0.0;
            }
            else
            {
                velMsg.omega = PID_GetOutput(&yawVelPid);
            }

            // printf("%f, ",velMsg.omega);
        }
        // printf("%d\r\n",chassisMode);

        xQueueOverwrite(chassisYawVelMsgBoxHandle, &velMsg);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisYunsuExe(void *argument)
{
    float lpfVel = 0.0;
    float lpfVelX = 0.0;
    float lpfVelY = 0.0;
    float logVel = 0.0;
    const float posDeadzone = 0.8;
    // float lpfOmega = 0.0;
    TickType_t xLastWakeTime = xTaskGetTickCount();
    VelMsg_t TarVelMsg = {0};

    /* 两种不同的走点 */
    // for (const int16_t period = 5; ;)
    for (const int16_t period = 20; ;)
    {
        VelMsg_t velMsg = {0};
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
        LocaterMsg_t locvelMsg;
        xQueuePeek(locaterVelMsgBoxHandle, &locvelMsg, portMAX_DELAY);
        if (chassisMode == CHASSIS_Front_MODE || chassisMode == CHASSIS_Back_MODE || chassisMode == CHASSIS_Left_MODE || chassisMode == CHASSIS_Right_MODE || chassisMode == CHASSIS_Right_Front_MODE || chassisMode == CHASSIS_Right_Back_MODE)
        {
            /*匀速走两种尝试-1:利用码盘数据进行闭环-2:每一段时间给走点增加*/
            if(chassisMode == CHASSIS_Front_MODE)
            {
                TarVelMsg.omega = 0;
                TarVelMsg.vel = 0.6;
                // sCurve0.speedTar += 0.0015;
                // sCurve1.speedTar += 0;
            }
            else if(chassisMode == CHASSIS_Back_MODE)
            {
                TarVelMsg.omega = 180;
                TarVelMsg.vel = 0.6;
                // sCurve0.speedTar -= 0.0015;
                // sCurve1.speedTar += 0;
            }
            else if(chassisMode == CHASSIS_Left_MODE)
            {
                TarVelMsg.omega = 90;
                TarVelMsg.vel = 0.6;
                // sCurve0.speedTar += 0;
                // sCurve1.speedTar += 0.0015;
            }
            else if(chassisMode == CHASSIS_Right_MODE)
            {
                TarVelMsg.omega = 270;
                TarVelMsg.vel = 0.6;
                // sCurve0.speedTar += 0;
                // sCurve1.speedTar -= 0.0015;
            }
            else if(chassisMode == CHASSIS_Right_Front_MODE)
            {
                TarVelMsg.omega = -45;
                TarVelMsg.vel = 0.6;
                // sCurve0.speedTar += 0.001;
                // sCurve1.speedTar -= 0.001;
            }
            else if(chassisMode == CHASSIS_Right_Back_MODE)
            {
                // sCurve0.speedTar -= 0.001;
                // sCurve1.speedTar -= 0.001;
                TarVelMsg.vel = 0.6;
                TarVelMsg.omega = -135;
            }
            // TarVelMsg.vel = 0.2;

            /*方法1*/
            PID_SetTarget(&VelPid, TarVelMsg.vel);

            logVel = locvelMsg.velX * cosf(TarVelMsg.omega / 180.0 * M_PI) + locvelMsg.velY * sinf(TarVelMsg.omega / 180.0 * M_PI) ;
            PID_Calculate(&VelPid, logVel);

            lpfVel = PID_GetOutput(&VelPid);
            lpfVelX = lpfVel * cosf(TarVelMsg.omega / 180.0 * M_PI);
            lpfVelY = lpfVel * sinf(TarVelMsg.omega / 180.0 * M_PI);
            velMsg.velX = lpfVelX * cosf(locMsg.posYaw / 180.0 * M_PI) + lpfVelY * sinf(locMsg.posYaw / 180.0 * M_PI);
            velMsg.velY = (-1) * lpfVelX * sinf(locMsg.posYaw / 180.0 * M_PI) + lpfVelY * cosf(locMsg.posYaw / 180.0 * M_PI);

            // printf("%f,%f,%f,%f\n", locvelMsg.velX, locvelMsg.velY, TarVelMsg.velX,velMsg.velX);
            // printf("%f,%f,%f,%f\n", locvelMsg.velX, locvelMsg.velY, velMsg.velX,velMsg.velY);
            printf("%f,%f,%f\n",logVel ,TarVelMsg.vel,lpfVel);

            /*方法2*/
            // SCurveProcess(&sCurve0);
            // SCurveProcess(&sCurve1);
            // float errX = sCurve0.out - locMsg.posX;
            // float errY = sCurve1.out - locMsg.posY;
            // float distance = -hypotf(errX, errY);
            // PID_Calculate(&pointPid, distance);
            // float v = PID_GetOutput(&pointPid);
            // float rawX = v * cosf(atan2f(errY, errX));
            // float rawY = v * sinf(atan2f(errY, errX));
            // velMsg.velX = rawX * cosf(locMsg.posYaw / 180.0 * M_PI) + rawY * sinf(locMsg.posYaw / 180.0 * M_PI);
            // velMsg.velY = (-1) * rawX * sinf(locMsg.posYaw / 180.0 * M_PI) + rawY * cosf(locMsg.posYaw / 180.0 * M_PI);

            const float posErr = fabs(locMsg.posYaw - PID_GetTarget(&yawPosPid));

            PID_Calculate(&yawPosPid, locMsg.posYaw);
            if (posErr <= posDeadzone)
            {
                PID_SetTarget(&yawVelPid, 0.0);
            }
            else
            {
                PID_SetTarget(&yawVelPid, PID_GetOutput(&yawPosPid));
            }

            PID_Calculate(&yawVelPid, locMsg.velYaw);
            if (posErr <= posDeadzone)
            {
                velMsg.omega = 0.0;
            }
            else
            {
                velMsg.omega = PID_GetOutput(&yawVelPid);
            }
            // printf("%d,%f,%f\r\n",chassisMode,velMsg.velX,velMsg.velY);
        }
        // printf("%d,%f,%f\r\n",chassisMode,sCurve0.speedTar,velMsg.velY);
        xQueueOverwrite(chassisYunsuVelMsgBoxHandle, &velMsg);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisSendVel(void *argument)
{
    float velX = 0.0;
    float velY = 0.0;
    float omega = 0.0;

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        VelMsg_t joyVel;
        xQueuePeek(chassisJoyVelMsgBoxHandle, &joyVel, portMAX_DELAY);

        VelMsg_t pointVel;
        xQueuePeek(chassisPointVelMsgBoxHandle, &pointVel, portMAX_DELAY);

        VelMsg_t yawVel;
        xQueuePeek(chassisYawVelMsgBoxHandle, &yawVel, portMAX_DELAY);

        VelMsg_t YunsuVel;
        xQueuePeek(chassisYunsuVelMsgBoxHandle, &YunsuVel, portMAX_DELAY);

        velX = joyVel.velX + pointVel.velX  + YunsuVel.velX;
        velY = joyVel.velY + pointVel.velY  + YunsuVel.velY;
        omega = joyVel.omega ;
        // omega = joyVel.omega + YunsuVel.omega;
        // omega = joyVel.omega + yawVel.omega + YunsuVel.omega;


        uint8_t buf[16] = {0};
        memcpy(buf + 0, &velX, sizeof(float));
        memcpy(buf + 4, &velY, sizeof(float));
        memcpy(buf + 8, &omega, sizeof(float));
        BSP_FDCAN_SendData(&hfdcan2, 0x100, buf, FDCAN_DLC_BYTES_16);

        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

void StartChassisCmdDeal(void *argument)
{
    PoseMsg_t posMsg;
    uint8_t point_allow_flag = 0;
    for (;;)
    {
        ChassisCmdMsg_t cmd;

        if (xQueueReceive(chassisCmdQueueHandle, &cmd, 5) == pdTRUE)
        {
            if (cmd == CHASSIS_FREE)
            {
                BSP_FDCAN_SendData(&hfdcan2, 0x160, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == CHASSIS_LOCK)
            {
                BSP_FDCAN_SendData(&hfdcan2, 0x161, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == CHASSIS_CONF_YAW)
            {
                chassisMode = YAW_MODE;
            }
            else if (cmd == CHASSIS_CONF_JOY)
            {
                chassisMode = JOY_MODE;
            }
            else if (cmd == CHASSIS_CONF_POINT)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveSoftProcess(&sCurve0,locMsg.posX);
                SCurveSoftProcess(&sCurve1,locMsg.posY);
                // PID_SetTarget(&yawPosPid, locMsg.posYaw);
                chassisMode = POINT_MODE;
            }
            else if (cmd == CHASSIS_Front)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveSoftProcess(&sCurve0,locMsg.posX);
                SCurveSoftProcess(&sCurve1,locMsg.posY);
                PID_SetTarget(&yawPosPid,0);
                chassisMode = CHASSIS_Front_MODE;
            }
            else if (cmd == CHASSIS_Back)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveSoftProcess(&sCurve0,locMsg.posX);
                SCurveSoftProcess(&sCurve1,locMsg.posY);
                PID_SetTarget(&yawPosPid,0);
                chassisMode = CHASSIS_Back_MODE;
            }
            else if (cmd == CHASSIS_Left)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveSoftProcess(&sCurve0,locMsg.posX);
                SCurveSoftProcess(&sCurve1,locMsg.posY);
                PID_SetTarget(&yawPosPid,0);
                chassisMode = CHASSIS_Left_MODE;
            }
            else if (cmd == CHASSIS_Right)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveSoftProcess(&sCurve0,locMsg.posX);
                SCurveSoftProcess(&sCurve1,locMsg.posY);
                PID_SetTarget(&yawPosPid,0);
                chassisMode = CHASSIS_Right_MODE;
            }
            else if (cmd == CHASSIS_Right_Front)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveSoftProcess(&sCurve0,locMsg.posX);
                SCurveSoftProcess(&sCurve1,locMsg.posY);
                PID_SetTarget(&yawPosPid,0);
                chassisMode = CHASSIS_Right_Front_MODE;
            }
            else if (cmd == CHASSIS_Right_Back)
            {
                LocaterMsg_t locMsg;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
                SCurveSoftProcess(&sCurve0,locMsg.posX);
                SCurveSoftProcess(&sCurve1,locMsg.posY);
                PID_SetTarget(&yawPosPid,0);
                chassisMode = CHASSIS_Right_Back_MODE;
            }
        }
        /* TODO: 拆开XY和YAW */
        if(chassisMode == POINT_MODE)
        {
            if (xQueueReceive(chassisPoseCmdQueueHandle, &posMsg, 5) == pdTRUE )
            {
                // sCurve0.speedTar = posMsg.posX;
                // sCurve1.speedTar = posMsg.posY;

                startPoint = endPoint;

                float dX = posMsg.posX - startPoint.posX;
                float dY = posMsg.posY - startPoint.posY;
                dTheta = atan2f(dY, dX);
                SCurveHardProcess(&sCurveM, 0.0);
                sCurveM.speedTar = hypotf(dX, dY);

                endPoint = posMsg;

                PID_SetTarget(&yawPosPid, posMsg.yaw);
            }
        }
        // printf("%d\r\n",chassisMode);
    }
}

void StartLocMerge(void *argument)
{

    TickType_t xLastWakeTime = xTaskGetTickCount();
    KFP Mapan_x_Kalman = {0.02,0,0,0,0.001,0.543};
    KFP Mapan_y_Kalman = {0.02,0,0,0,0.001,0.543};
    KFP Radar_x_Kalman = {0.02,0,0,0,0.001,0.543};
    KFP Radar_y_Kalman = {0.02,0,0,0,0.001,0.543};

    for (const int16_t period = 5; ;)
    {
        LocaterMsg_t Radar_locMsg;
        xQueuePeek(radarLocHandle, &Radar_locMsg, portMAX_DELAY); // 读取雷达原始数据
        // 将雷达原始数据变换到底盘中心
        Radar_locMsg.posX = Radar_locMsg.posX + 0.256 - 0.256 * cosf(Radar_locMsg.posYaw);
        Radar_locMsg.posY = Radar_locMsg.posY - 0.256 * sinf(Radar_locMsg.posYaw);

        LocaterMsg_t Mapan_locMsg;
        xQueuePeek(locaterMsgBoxHandle, &Mapan_locMsg, portMAX_DELAY);

        LocaterMsg_t Merge_locMsg;
        Merge_Loc_X= kalmanFilter_Merge(&Mapan_x_Kalman,&Radar_x_Kalman,Mapan_locMsg.posX,Radar_locMsg.posX);
        Merge_Loc_Y = kalmanFilter_Merge(&Mapan_y_Kalman,&Radar_y_Kalman,Mapan_locMsg.posY,Radar_locMsg.posY);

        // float dis, yaw;
        //
        // chassisMode = POINT_MODE;


        // printf("%f,%f,%f,%f,%f,%f\r\n", Radar_locMsg.posX,Mapan_locMsg.posX,Merge_Loc_X,Radar_locMsg.posY,Mapan_locMsg.posY,Merge_Loc_Y);
        vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(period));
    }
}

Locater_t last_locMsg = {0};
void StartchassisVelCal(void *argument)
{
    LocaterMsg_t locMsg_Fliter = {0};
    for (const int16_t period = 20; ;)
    {
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
        locMsg.velX = (locMsg.posX - last_locMsg.posX)  * 50.0;
        locMsg.velY = (locMsg.posY - last_locMsg.posY)  * 50.0;
        locMsg_Fliter.velX += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.020 * 50))) * (locMsg.velX  - locMsg_Fliter.velX);    // 采样周期20ms, 截止频率50Hz
        locMsg_Fliter.velY += (1.0 / (1.0 + 1.0 / (2.0 * M_PI * 0.020 * 50))) * (locMsg.velY  - locMsg_Fliter.velY);    // 采样周期20ms, 截止频率50Hz
        xQueueOverwrite(locaterVelMsgBoxHandle,&locMsg_Fliter);
        last_locMsg.posX = locMsg.posX;
        last_locMsg.posY = locMsg.posY;
        vTaskDelay(pdMS_TO_TICKS(period));
     }
}

void StartChassisPrintf(void *argument)
{
    // vTaskSuspend(NULL);
    for (const int16_t period = 20; ;)
    {
        PidArgsMsg_t msg;
        if (xQueueReceive(pidArgsQueueHandle, &msg, 5) == pdTRUE)
        {
            PID_SetArgs(&VelPid, msg.kp, msg.ki, msg.kd);
            PID_SetTarget(&VelPid, msg.target);
        }
        // printf("111\r\n");

        // LocaterMsg_t locMsg;
        // xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
        // printf("%f, %f, %f, %f, %f\r\n", sCurve0.out,sCurve1.out,locMsg.posX, locMsg.posY, locMsg.velYaw);
        // printf("%f, %f, %f\r\n", locMsg.posX, locMsg.posY, locMsg.velYaw);

        // KeyCmdMsg_t key_cmd_msg;
        // xQueueReceive(keyCmdQueueHandle,&key_cmd_msg,portMAX_DELAY);
        // printf("%d\r\n",key_cmd_msg);
        vTaskDelay(pdMS_TO_TICKS(period));
    }
}
